#include <stdlib.h>
#include "RSA_common.h"

#include <string.h>
#if PROCESSOR == INTEL
#include <iostream>
using namespace std;
#endif

#if PROCESSOR == INTEL
static const uint8_t hexadecimal_alphabet[] = {
  '0', '1', '2', '3',
  '4', '5', '6', '7',
  '8', '9', 'A', 'B',
  'C', 'D', 'E', 'F'
};
#endif

/* General-purpose memory */
#define GPM_ARRAY_LENGTH 6 /* Adjust in accordance with the actual needs */
#define GPM_ELEMENT_SIZE (2 * KEY_SZ / 32)
static uint32_t GPM[GPM_ARRAY_LENGTH][GPM_ELEMENT_SIZE]; 

// 160 <- "a0"
uint8_t to_uint8_t(char *src)
{
  uint8_t value = 0;
  
  for (int i = 0; i < 2; i++)
  {
    const char c = src[i];
    if (c >= '0' && c <= '9')
    {
      const uint8_t u = (c - '0') << (4*(2 - i - 1)); // do not put this expression directly in the RHS below
      value |= u;
    }
    else
    {
      if (c >= 'a' && c <= 'f')
      {
        const uint8_t u = (c - 'a' + 10) << (4*(2 - i - 1));
        value |= u;
      }
      else
      {
#if PROCESSOR == INTEL
        throw new runtime_error("bad character");
#endif
      }
    }
  }
  
  return value;
}

void multi__reset(uint32_t *value, uint32_t size)
{
  uint32_t i;
  
  for (i = 0; i < size; i++)
  {
    value[i] = 0;
  }
}

void multi__set(void *dest, char *src)
{
  const int len = strlen(src);
  if ((len & 1) == 0/*len must be even*/)
  {
    uint8_t *ptr = (uint8_t*)dest;
    
    for (signed int i = len - 2; i >= 0; i -= 2)
    {
      char *s = src + i;
      ptr[(len - i) / 2 - 1] = to_uint8_t(s);
    }
  }
  else
  {
#if PROCESSOR == INTEL
    throw new runtime_error("src length is odd");
#else
    asm("nop");
#endif
  }
}

#if PROCESSOR == INTEL
void print_uint8_t(uint8_t value)
{
  cout << hexadecimal_alphabet[value >> 4] << hexadecimal_alphabet[value & 15];
}
#endif


#if PROCESSOR == INTEL
void multi__print(uint32_t *value)
{
  uint8_t *ptr = (uint8_t*)value;
  bool skip_zero = true;
  
  /* print each octet, starting from most significant */
  for (signed int i = (KEY_SIZE / 8) - 1; i >= 0; i--)
  {
    if (skip_zero)
    {
      if (ptr[i] != 0)
      {
        print_uint8_t(ptr[i]);
        skip_zero = false;
      }
    }
    else
    {
      print_uint8_t(ptr[i]);
    }
  }
}
#endif

extern uint32_t low_level__multi__mul_helper(uint32_t x, uint32_t y, uint32_t z, uint32_t *carry);

// lang: C
// test: PASS (with Java Math.BigInteger)
// For multiword arithmetic examples in assembler see Cortex-M3 prog. man. at pag. 73-74.
// Cannot overflow.
// Fault source: if the expression for "prod" contains only 32-bit integers then high qword of "prod" is zero
void multi__mul(uint32_t *result, uint32_t *a, uint32_t *b, uint32_t size)
{
  uint32_t i; /* iterator */
  uint32_t j; /* iterator */
  
  multi__reset(result, 2 * size);
  
  uint32_t x, y, z;
  uint32_t carry;
  uint64_t prod;
  
  for (i = 0; i < size; i++)
  {
    x = a[i];
    carry = 0;
    for (j = 0; j < size; j++)
    {
      y = b[j];
      z = result[i + j];
#if 1
      prod = (uint64_t)x*(uint64_t)y + (uint64_t)z + (uint64_t)carry;
      result[i + j] = prod & 4294967295L; /* prod LOW */
      carry = prod >> 32; /* prod HIGH */
#endif
#if 0 /* This alternative uses the UMLAL instruction.
         It is not selected because the compiler already uses the UMLAL instruction... 
         The compiler is very efficient. */
      result[i + j] = low_level__multi__mul_helper(x, y, z, &carry);
#endif
    }
    result[i + j] = carry;
  }
}

void multi__lshift(uint32_t *objData, uint32_t objSize, uint32_t distance)
{
  int32_t i;
  const int32_t block_distance = distance / 32; // quotient
  const int32_t subblock_distance = distance & (32 - 1); // remainder
  
  for (i = objSize - 1; i >= 0; i--)
  {
    const int32_t j = i - block_distance;
    if (j >= 0)
    {
      objData[i] = objData[j] << subblock_distance;
      const int32_t k = j - 1;
      if (k >= 0)
      {
        objData[i] |= objData[k] >> (32 - subblock_distance);
      }
    }
    else
    {
      objData[i] = 0;
    }
  }
}

void multi__rshift1(void *value, uint32_t size)
{
#if PROCESSOR == INTEL
  throw new runtime_error("Not implemented");
#else
  low_level__multi__rshift1(value, size);
#endif
}

/* If difference is negative then zero is returned, otherwise nonzero. */
uint32_t multi__sub(uint32_t *result, uint32_t *a, uint32_t *b, uint32_t size)
{
#if PROCESSOR == INTEL
  throw new runtime_error("Not implemented");
#else
  return low_level__multi__sub(result, a, b, size);
#endif
}

uint32_t effective_bit_length(uint32_t *objData, uint32_t objSize)
{
#if PROCESSOR == INTEL
  throw new runtime_error("Not implemented");
#else
  uint32_t pos = 0;
  int32_t i;
  
  /* count down */
  for (i = objSize - 1; i >= 0; i--)
  {
    const uint32_t value = objData[i];
    const uint32_t z = low_level__clz(value);
    if (z < 32/* nonzero value */)
    {
      pos = (32 - z) + (32*i);
      break;
    }
  }
  
  return pos;
#endif
}

/* N/D = Q + R */
void multi__div(uint32_t *R/* objSize */, uint32_t *Q, uint32_t *N, uint32_t *D, uint32_t objSize)
{
  int32_t i;
  
  const uint32_t effective_bit_length_N = effective_bit_length(N, objSize);
  const uint32_t effective_bit_length_D = effective_bit_length(D, objSize);
  const  int32_t effective_bit_length_Q = (int32_t)effective_bit_length_N 
    - (int32_t)effective_bit_length_D;
  
  multi__reset(Q, objSize);
  
  /* set Difference */
  multi__copy(GPM[0], N, objSize);
  
  /* set substract value */
  multi__copy(GPM[1], D, objSize);
  multi__lshift(GPM[1], objSize, effective_bit_length_Q);
  
  // as vrea sa arate un pic altfel, dar s-ar parea ca functioneaza perfect in
  // varianta actuala asa ca vezi alte probleme

  for (i = effective_bit_length_Q; i >= 0; i--)
  {
    const uint32_t carry = multi__sub(GPM[2], GPM[0], GPM[1], objSize);
    if (carry != 0/* GPM[0] >= GPM[1] */)
    {
      /* set bit Q[i] */
      Q[i >> 5/* i / 32 */] |= (1 << (i & 31/* i % 32 */));
      
      /* change Difference */
      multi__copy(GPM[0], GPM[2], objSize);
    }
    else
    {
      /* let bit Q[i] 0 */
    }
    
    multi__rshift1(GPM[1], objSize);
  }
  
  /* copy Difference to R */
  multi__copy(R, GPM[0], objSize);
}

/*
Operation: R = remainder of N/D
Note: This is shorter variant of 'multi__div'
*/
void multi__rem(uint32_t *R, uint32_t *N, uint32_t *D, uint32_t size)
{
  /* Only size is used from GPM sections. */
  
  int32_t i;
  
  const uint32_t effective_bit_length_N = effective_bit_length(N, size);
  const uint32_t effective_bit_length_D = effective_bit_length(D, size);
  const  int32_t effective_bit_length_Q = (int32_t)effective_bit_length_N 
    - (int32_t)effective_bit_length_D;
  
  /* set Difference */
  low_level__multi__copy(GPM[0], N, size);
  
  /* set substract value */
  low_level__multi__copy(GPM[1], D, size);
  multi__lshift(GPM[1], size, effective_bit_length_Q);
  
  // as vrea sa arate un pic altfel, dar s-ar parea ca functioneaza perfect in
  // varianta actuala asa ca vezi alte probleme

  for (i = effective_bit_length_Q; i >= 0; i--)
  {
    const uint32_t carry = multi__sub(GPM[2], GPM[0], GPM[1], size);
    if (carry != 0/* GPM[0] >= GPM[1] */)
    {
      /* change Difference */
      low_level__multi__copy(GPM[0], GPM[2], size);
    }
    
    multi__rshift1(GPM[1], size);
  }
  
  /* copy Difference to R */
  low_level__multi__copy(R, GPM[0], size);
}

void multi__copy(uint32_t *dest, uint32_t *src, uint32_t size)
{
#if 0
  uint32_t i;
  
  for (i = 0; i < size; i++)
  {
    dest[i] = src[i];
  }
#endif
#if 1
  low_level__multi__copy(dest, src, size);
#endif
}

/* GPM usage: 3, 4, 5 */
void modular_pow(uint32_t *dest, uint32_t *base, uint32_t exp, uint32_t *modulus, uint32_t size)
{
  /* result = 1; */
  multi__reset(dest, size);
  dest[0] = 1;
  
  /* Do not modify base. Operate with GPM[3] section as base. */
  multi__copy(GPM[3], base, size);
  
  /* Use GPM[4] as multiplication result. */
  
  while (exp > 0)
  {
    if (exp & 1)
    {
      multi__mul(GPM[4], dest, GPM[3], size);
      multi__rem(dest, GPM[4], modulus, 2 * size);
    }
    exp >>= 1;
    
    multi__mul(GPM[4], GPM[3], GPM[3], size);
    multi__rem(GPM[3], GPM[4], modulus, 2 * size);
  }
}

/* size can be 32, 64, 128, 256 and so on until KEY_SIZE */
/* Fermat */
flag_t is_composite(uint32_t *value, uint32_t size)
{
  static uint32_t n[KEY_SZ / 32];
  
  /* n = 1 */
  multi__reset(n, KEY_SZ / 32);
  n[0] = 1;
  
  /* n = value - 1 */
  multi__sub(n, value, n, size);
  
  uint32_t i;
  
  for (i = 0; i < STRENGTH; i++)
  {
    /* Generate random a */
    static uint32_t a[KEY_SZ / 32];
    multi__reset(a, KEY_SZ / 32);
    multi__limited_rand(a, n, size);
    
    static uint32_t r[KEY_SZ / 32];
    multi__reset(r, KEY_SZ / 32);
    
    modular_pow__longexp(r, a, n, value, size);
    
    if (is_one(r, KEY_SZ / 32) == NO)
    {
      return YES; /* Is composite */
    }
  }
  
  return NO; /* Is probable prime */
}


static uint32_t long__rand(void)
{
  const uint32_t r0 = rand();
  const uint32_t r1 = rand();
  return (r1 << 16) | r0;
  /* Bits 15 and 31 are 0 actually */
}

void multi__limited_rand(uint32_t *dest, uint32_t *high, uint32_t size)
{
  int32_t i;
  flag_t flag = NO;
  
  /* From High to Low */
  for (i = size - 1; i >= 0; i--)
  {
    if (NO == flag)
    {
      if (0 == high[i])
      {
        /* Do nothing */
      }
      else
      {
        /* Highest nonzero 32-bit value */
        flag = YES;
        
        /* Generate random value under high[i] */
        const uint32_t r = long__rand();
        dest[i] = r % high[i];
      }
    }
    else
    {
      dest[i] = long__rand();
    }
  }
}

void multi__rand(uint32_t *dest, uint32_t size)
{
  uint32_t i;
  
  /* From High to Low */
  for (i = 0; i < size; i++)
  {
    dest[i] = long__rand();
  }
}

/* to do: test (it is tested only indirectly) */
void modular_pow__longexp(uint32_t *dest, uint32_t *base, uint32_t *exp, uint32_t *modulus, uint32_t size)
{
  uint32_t i;
  
  /* result = 1; */
  multi__reset(dest, size);
  dest[0] = 1;
  
  /* Do not modify base. Operate with GPM[3] section as base. */
  multi__copy(GPM[3], base, size);
  
  /* Use GPM[4] as multiplication result. */
  
  static uint32_t exp_buf[KEY_SZ / 32];
  multi__reset(exp_buf, KEY_SZ / 32);
  multi__copy(exp_buf, exp, size);
  
  const uint32_t n = effective_bit_length(exp_buf, size);
  
  for (i = 0; i < n; i++)
  {
    if (exp_buf[0] & 1)
    {
      multi__mul(GPM[4], dest, GPM[3], size);
      multi__rem(dest, GPM[4], modulus, 2 * size);
    }
    multi__rshift1(exp_buf, size);
    
    multi__mul(GPM[4], GPM[3], GPM[3], size);
    multi__rem(GPM[3], GPM[4], modulus, 2 * size);
  }
}

void generate_probable_prime(uint32_t *dest, uint32_t size)
{
  flag_t flag = YES;
  
  /* Generate random odd value */
  multi__rand(dest, size);
  dest[0] |= 1;
  
  while (YES == flag)
  {
    if (is_composite(dest, size) == YES)
    {
      /* Add 2 */
      multi__add_2(dest, size);
    }
    else
    {
      /* Return */
      flag = NO;
    }
  }
}

void multi__add_2(uint32_t *value, uint32_t size)
{
  uint32_t i, data;
  
  data = value[0];
  value[0] += 2;
  if (value[0] < data)
  {
    /* Carry */
    for (i = 1; i < size / 32; i++)
    {
      data = value[i];
      value[i] += 1;
      if (value[i] > data)
      {
        /* No carry. Stop. */
        break;
      }
    }
  }
}

// algoritmul este luat de aici: http://www.di-mgt.com.au/euclidean.html
void multi__modular_inverse(uint32_t *result, uint32_t *u, uint32_t *v, uint32_t size)
{
  static uint32_t u1[2 * KEY_SZ / 32], u3[2 * KEY_SZ / 32];
  static uint32_t v1[2 * KEY_SZ / 32], v3[2 * KEY_SZ / 32];
  static uint32_t t1[2 * KEY_SZ / 32], t3[2 * KEY_SZ / 32];
  static uint32_t  q[2 * KEY_SZ / 32];
  
  int32_t i;
  
  /* u1 = 1 */
  multi__reset(u1, 2 * KEY_SZ / 32);
  u1[0] = 1;
  
  /* u3 = u */
  multi__reset(u3, 2 * KEY_SZ / 32);
  multi__copy(u3, u, size);
  
  /* v1 = 0 */
  multi__reset(v1, 2 * KEY_SZ / 32);
  
  /* v3 = v */
  multi__reset(v3, 2 * KEY_SZ / 32);
  multi__copy(v3, v, size);
  
  i = 1;
  
  while (is_zero(v3, size) == NO)
  {
    multi__div(t3, q, u3, v3, size);
    
    multi__mul(t1, q, v1, size);
    multi__add(t1, t1, u1, size);
    
    multi__copy(u1, v1, size);
    multi__copy(v1, t1, size);
    multi__copy(u3, v3, size);
    multi__copy(v3, t3, size);
    
    i = -i;
  }
  
  /* Ensure u3 = gcd(u,v) == 1 */
  if (is_one(u3, size) == NO)
  {
    /* Value not invertible */
    multi__reset(result, size);
    return;
  }
  
  if (i < 0)
  {
    multi__sub(result, v, u1, size);
  }
  else
  {
    multi__copy(result, u1, size);
  }
}

uint32_t is_zero(uint32_t *value, uint32_t size)
{
  int32_t i;
  
  for (i = 0; i < size; i++)
  {
    if (value[i] != 0)
    {
      return NO;
    }
  }
  
  return YES;
}

uint32_t is_one(uint32_t *value, uint32_t size)
{
  int32_t i;
  
  if (value[0] != 1)
  {
    return NO;
  }
  
  for (i = 1; i < size; i++)
  {
    if (value[i] != 0)
    {
      return NO;
    }
  }
  
  return YES;
}

uint32_t multi__add(uint32_t *result, uint32_t *a, uint32_t *b, uint32_t size)
{
#if PROCESSOR == INTEL
  throw new runtime_error("Not implemented");
#else
  return low_level__multi__add(result, a, b, size);
#endif
}
